feature_base.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Forward declarations for node templates
namespace wolf
{
class CaptureBase;
class FactorBase;
}  // namespace wolf
 
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
 
// std includes
 
namespace wolf
{
// class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
    friend CaptureBase;
    friend SensorBase;
    friend FactorBase;
 
  private:
    CaptureBaseWPtr   capture_ptr_;
    FactorBasePtrList factor_list_;
 
    static unsigned int feature_id_count_;
 
  protected:
    unsigned int    feature_id_;
    unsigned int    track_id_;                            // ID of the feature track
    unsigned int    landmark_id_;                         // ID of the landmark
    Eigen::VectorXd measurement_;                         
    Eigen::MatrixXd measurement_covariance_;              
    Eigen::MatrixXd measurement_sqrt_information_upper_;  
    Eigen::VectorXd expectation_;                         
 
  public:
    typedef enum
    {
        UNCERTAINTY_IS_COVARIANCE,
        UNCERTAINTY_IS_INFO,
        UNCERTAINTY_IS_STDDEV
    } UncertaintyType;
 
    FeatureBase(const std::string&     _type,
                const Eigen::VectorXd& _measurement,
                const Eigen::MatrixXd& _meas_uncertainty,
                UncertaintyType        _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
 
    ~FeatureBase() override = default;
    virtual void remove(bool viral_remove_parent_without_children = false);
    bool         hasChildren() const override
    {
        return not factor_list_.empty();
    };
 
    // properties
    unsigned int id() const;
    unsigned int trackId() const
    {
        return track_id_;
    }
    void setTrackId(unsigned int _tr_id)
    {
        track_id_ = _tr_id;
    }
    unsigned int landmarkId() const
    {
        return landmark_id_;
    }
    void setLandmarkId(unsigned int _lmk_id)
    {
        landmark_id_ = _lmk_id;
    }
 
    // values
    /* \brief Returns _ii component of measurement vector
     *
     * WARNING: To be fast, it does not check that index _ii is smaller than dimension.
     */
    double                 getMeasurement(unsigned int _ii) const;
    const Eigen::VectorXd& getMeasurement() const;
 
  private:
    /* \brief Set the measurement and its noise
     *
     * WARNING: To be used once in the constructor only.
     */
    void setMeasurement(const Eigen::VectorXd& _meas);
    void setMeasurementCovariance(const Eigen::MatrixXd& _meas_cov);
    void setMeasurementInformation(const Eigen::MatrixXd& _meas_info);
 
  public:
    const Eigen::MatrixXd& getMeasurementCovariance() const;
    Eigen::MatrixXd        getMeasurementInformation() const;
    const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
 
    const Eigen::VectorXd& getExpectation() const;
    void                   setExpectation(const Eigen::VectorXd& expectation);
 
    // wolf tree access
    FrameBaseConstPtr getFrame() const;
    FrameBasePtr      getFrame();
 
    CaptureBaseConstPtr getCapture() const;
    CaptureBasePtr      getCapture();
 
    FactorBaseConstPtrList getFactorList() const;
    FactorBasePtrList      getFactorList();
    void                   getFactorList(FactorBaseConstPtrList& _fac_list) const;
    void                   getFactorList(FactorBasePtrList& _fac_list);
    bool                   hasFactor(FactorBaseConstPtr _fac) const;
 
    void link(CaptureBasePtr cap_ptr);
    template <typename classType, typename... T>
    static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
 
    virtual void printHeader(int           depth,        //
                             bool          factored_by,  //
                             bool          metric,       //
                             bool          state_blocks,
                             std::ostream& stream,
                             std::string   _tabs = "") const;
    void         print(int           depth,        //
                       bool          factored_by,  //
                       bool          metric,       //
                       bool          state_blocks,
                       std::ostream& stream = std::cout,
                       std::string   _tabs  = "") const;
 
    virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
    bool             check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
 
  private:
    void setCapture(CaptureBasePtr _cap_ptr)
    {
        capture_ptr_ = _cap_ptr;
    }
    FactorBasePtr addFactor(FactorBasePtr _co_ptr);
    void          removeFactor(FactorBasePtr _co_ptr);
};
 
}  // namespace wolf
 
// IMPLEMENTATION
 
#include "core/factor/factor_base.h"
 
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
{
    std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
    ftr->link(_cpt_ptr);
    return ftr;
}
 
inline FactorBaseConstPtrList FeatureBase::getFactorList() const
{
    FactorBaseConstPtrList list_const;
    for (auto&& obj_ptr : factor_list_) list_const.push_back(obj_ptr);
    return list_const;
}
 
inline FactorBasePtrList FeatureBase::getFactorList()
{
    return factor_list_;
}
 
inline unsigned int FeatureBase::id() const
{
    return feature_id_;
}
 
inline CaptureBaseConstPtr FeatureBase::getCapture() const
{
    return capture_ptr_.lock();
}
 
inline CaptureBasePtr FeatureBase::getCapture()
{
    return capture_ptr_.lock();
}
 
inline double FeatureBase::getMeasurement(unsigned int _ii) const
{
    return measurement_(_ii);
}
 
inline const Eigen::VectorXd& FeatureBase::getMeasurement() const
{
    return measurement_;
}
 
inline const Eigen::MatrixXd& FeatureBase::getMeasurementCovariance() const
{
    return measurement_covariance_;
}
 
inline Eigen::MatrixXd FeatureBase::getMeasurementInformation() const
{
    return measurement_sqrt_information_upper_.transpose() * measurement_sqrt_information_upper_;
}
 
inline const Eigen::MatrixXd& FeatureBase::getMeasurementSquareRootInformationUpper() const
{
    return measurement_sqrt_information_upper_;
}
 
inline const Eigen::VectorXd& FeatureBase::getExpectation() const
{
    return expectation_;
}
 
inline void FeatureBase::setExpectation(const Eigen::VectorXd& expectation)
{
    expectation_ = expectation;
}
 
}  // namespace wolf